/*
 * GuiWrapper.cpp
 *
 *  Created on: 4 févr. 2010
 *      Author: labm2414
 */

#include "GuiWrapper.h"
#include "MsgConversion.h"
#include <QtGui/QApplication>

#include <cv_bridge/cv_bridge.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Empty.h>

#include <utilite/UEventsManager.h>
#include <utilite/UImageEvent.h>

#include <opencv2/highgui/highgui.hpp>

#include <semol/gui/MainWindow.h>
#include <semol/core/SemolEvent.h>
#include <semol/core/ParamEvent.h>
#include <semol/core/Parameters.h>
#include <semol/core/Sensor.h>
#include <semol/core/SensorimotorEvent.h>

#include "PreferencesDialogROS.h"

using namespace semol;

GuiWrapper::GuiWrapper(int & argc, char** argv)
{
	ros::NodeHandle nh;
	infoExTopic_ = nh.subscribe("semol/infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
	velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
	app_ = new QApplication(argc, argv);
	mainWindow_ = new MainWindow(new PreferencesDialogROS());
	mainWindow_->show();
	mainWindow_->changeState(MainWindow::kMonitoring);
	app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );

	resetMemoryClient_ = nh.serviceClient<std_srvs::Empty>("semol/resetMemory");
	dumpMemoryClient_ = nh.serviceClient<std_srvs::Empty>("semol/dumpMemory");
	dumpPredictionClient_ = nh.serviceClient<std_srvs::Empty>("semol/dumpPrediction");
	deleteMemoryClient_ = nh.serviceClient<std_srvs::Empty>("semol/deleteMemory");

	nh = ros::NodeHandle("~");
	parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);

	UEventsManager::addHandler(this);
	UEventsManager::addHandler(mainWindow_);
}

GuiWrapper::~GuiWrapper()
{
	delete mainWindow_;
	delete app_;
}

int GuiWrapper::exec()
{
	return app_->exec();
}

void GuiWrapper::infoExReceivedCallback(const semol::InfoExConstPtr & msg)
{
	ROS_INFO("infoEx received!");

	// Map from ROS struct to semol struct
	semol::Statistics stat;

	stat.setExtended(true); // Extended

	stat.setRefImageId(msg->refId);
	std::list<SensorRaw> sensors;
	for(unsigned int i=0; i<msg->refRawData.size(); ++i)
	{
		SensorRaw s((Sensor::Type)msg->refRawData[i].type, fromCvMatMsgToCvMat(msg->refRawData[i].matrix));
		if(s.data().total())
		{
			sensors.push_back(s);
		}
	}
	stat.setRefRawData(sensors);

	stat.setLoopClosureId(msg->loopClosureId);
	sensors.clear();
	for(unsigned int i=0; i<msg->loopRawData.size(); ++i)
	{
		SensorRaw s((Sensor::Type)msg->loopRawData[i].type, fromCvMatMsgToCvMat(msg->loopRawData[i].matrix));
		if(s.data().total())
		{
			sensors.push_back(s);
		}
	}
	stat.setLoopClosureRawData(sensors);

	// sensor post data
	std::list<std::vector<float> > postSensors;
	for(unsigned int i=0; i<msg->refSensorData.size(); ++i)
	{
		cv::Mat m = fromCvMatMsgToCvMat(msg->refSensorData[i]);
		if(m.rows == 1 && m.cols > 0 && m.type() == CV_32FC1)
		{
			std::vector<float> descriptor(m.cols);
			memcpy(descriptor.data(), m.data, m.cols * sizeof(float));
			postSensors.push_back(descriptor);
		}
	}
	stat.setRefSensorData(postSensors);

	postSensors.clear();
	for(unsigned int i=0; i<msg->loopSensorData.size(); ++i)
	{
		cv::Mat m = fromCvMatMsgToCvMat(msg->loopSensorData[i]);
		if(m.rows == 1 && m.cols > 0 && m.type() == CV_32FC1)
		{
			std::vector<float> descriptor(m.cols);
			memcpy(descriptor.data(), m.data, m.cols * sizeof(float));
			postSensors.push_back(descriptor);
		}
	}
	stat.setLoopSensorData(postSensors);

	//Posterior, likelihood, childCount
	std::map<int, float> mapIntFloat;
	for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
	{
		mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
	}
	stat.setPosterior(mapIntFloat);
	mapIntFloat.clear();
	for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
	{
		mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
	}
	stat.setLikelihood(mapIntFloat);
	std::map<int, int> mapIntInt;
	for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
	{
		mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
	}
	stat.setWeights(mapIntInt);

	//Actions
	std::list<Actuator> actuators;
	for(unsigned int i=0; i<msg->actuators.size(); ++i)
	{
		Actuator a(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type);
		if(a.data().total())
		{
			actuators.push_back(a);
		}
	}
	stat.setActuators(actuators);

	// Statistics data
	for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
	{
		stat.addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
	}

	ROS_INFO("Publishing statistics...");
	UEventsManager::post(new semol::SemolEvent(stat));
}

void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg)
{
	cv::Mat data = cv::Mat(1, 6, CV_32F);
	data.at<float>(0) = (float)msg->twist.linear.x;
	data.at<float>(1) = (float)msg->twist.linear.y;
	data.at<float>(2) = (float)msg->twist.linear.z;
	data.at<float>(3) = (float)msg->twist.angular.x;
	data.at<float>(4) = (float)msg->twist.angular.y;
	data.at<float>(5) = (float)msg->twist.angular.z;

	std::list<Actuator> actuators;
	actuators.push_back(Actuator(data, semol::Actuator::kTypeTwist));
	this->post(new semol::SensorimotorEvent(std::list<SensorRaw>(), actuators));
}

void GuiWrapper::handleEvent(UEvent * anEvent)
{
	if(anEvent->getClassName().compare("ParamEvent") == 0)
	{
		const semol::ParametersMap & defaultParameters = semol::Parameters::getDefaultParameters();
		semol::ParametersMap parameters = ((semol::ParamEvent *)anEvent)->getParameters();
		bool modified = false;
		ros::NodeHandle nh("semol");
		for(semol::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
		{
			//save only parameters with valid names
			if(defaultParameters.find((*i).first) != defaultParameters.end())
			{
				nh.setParam((*i).first, (*i).second);
				modified = true;
			}
			else if((*i).first.find('/') != (*i).first.npos)
			{
				ROS_WARN("Parameter %s is not used by the semol node.", (*i).first.c_str());
			}
		}
		if(modified)
		{
			ROS_INFO("Parameters updated");
			parametersUpdatedPub_.publish(std_msgs::Empty());
		}
	}
	else if(anEvent->getClassName().compare("SemolEventCmd") == 0)
	{
		std_srvs::Empty srv;
		semol::SemolEventCmd::Cmd cmd = ((semol::SemolEventCmd *)anEvent)->getCmd();
		if(cmd == semol::SemolEventCmd::kCmdDumpMemory)
		{
			if(!dumpMemoryClient_.call(srv))
			{
				ROS_ERROR("Can't call \"dumpMemory\" service");
			}
		}
		else if(cmd == semol::SemolEventCmd::kCmdDumpPrediction)
		{
			if(!dumpPredictionClient_.call(srv))
			{
				ROS_ERROR("Can't call \"dumpPrediction\" service");
			}
		}
		else if(cmd == semol::SemolEventCmd::kCmdResetMemory)
		{
			if(!resetMemoryClient_.call(srv))
			{
				ROS_ERROR("Can't call \"resetMemory\" service");
			}
		}
		else if(cmd == semol::SemolEventCmd::kCmdDeleteMemory)
		{
			if(!deleteMemoryClient_.call(srv))
			{
				ROS_ERROR("Can't call \"deleteMemory\" service");
			}
		}
		else
		{
			ROS_WARN("Unknown command...");
		}
	}
}
